#include "Router.h"
#include <unordered_set>

// 算法部分============================================================================
void Router::main_router() {
	// 1.构建空白寻路地图（在数据读取中构建）
	// 2.禁布区预处理，产生角点CPoint（在类外部调用预处理函数）
	// 3.寻路，产生节点Node
	for (int i = 0; i < m_Pins.size(); i++) {
		NodePoint* pt1 = m_Pins.at(i).first;
		NodePoint* pt2 = m_Pins.at(i).second;
		// 起点和终点在禁布区内部，则忽略该禁布区，临时将该禁布区内的禁布点置为空白
		set_obs_and_access(pt1, pt2, accessSymbol);
		bool found = path_finding(pt1, pt2);		// 寻找路，生成树结构
		if (!found) {
			cout << "路径" << i << "未找到" << endl;
			continue;
		}
		else {	// 找到了路径，执行路径优化
			if (do_optimize_level)
				node_optimize(m_path[i]);			// 节点优化
			if (do_post_processing)
				main_post_processing(m_path[i]);	// 后处理模块
		}
		set_obs_and_access(pt1, pt2, obsSymbol);
		finding_data_clear(m_path_tree[i]);		//完成一条路径的寻路，清理相关数据，准备寻下一条路径
	}

	cout << "Routing over (without post),FAs_sum: " << m_obsArr.size() << "\tPin_sum: " << m_Pins.size() << endl;

	/*for (int i = 0; i < m_path.size(); i++) {
		cout << "路径 " << i << " 的长度：" << m_nodes[m_path[i]].F << "\t布线树的节点总数：" << m_nodes.size() << endl;
	}*/
}
// 禁布区预处理模块
void Router::obs_init() {
	int obsSum = 0;
	m_obs_pt_sum = 0;
	for (int i = 0; i < xSize; i++) {
		for (int j = 0; j < ySize; j++) {
			NodePoint& pt = (*m_ptArr)[i][j];
			if (pt.symbol == obsSymbol) {
				m_obs_pt_sum++;
				if (pt.obsIndex == -1)
					add_new_obstacle(pt, obsSum);
			}
		}
	}
}
bool Router::is_inside(int x, int y) const {
	return x >= 0 && x < xSize && y >= 0 && y < ySize;
}
void Router::add_new_obstacle(NodePoint& pt, int& obsSum) {
	//// 创建禁布区，添加第一层角节点，并对角节点沿轮廓排序，设置顺序值
	//// 0.预处理，设置禁布区编号，放入新建的 openList 中，准备进行广度优先搜索
	//std::vector<NodePoint*> extend_list;
	//std::vector<NodePoint*> obsList;
	//pt.obsIndex = obsSum;
	//extend_list.emplace_back(&pt);		//搜索过程中的外围点
	//obsList.emplace_back(&pt);		//禁布区所有点的集合
	//this->m_obsArr.emplace_back(Obstacles(obsSum));
	//Obstacles& obsObj = this->m_obsArr.back();	//第0个禁布区，暂时为空

	//// 1.将所有连在一起的禁布区点视作一个禁布区，将地图中点的这些点的 obsIndex 设为 -2，表示已探索过
	//while (!extend_list.empty()) {
	//	NodePoint& pt_center = *extend_list.back();
	//	extend_list.pop_back();
	//	for (const auto& dir : directions) {
	//		int neib_x = pt_center.x + dir.x;
	//		int neib_y = pt_center.y + dir.y;
	//		if (is_inside(neib_x, neib_y)) {
	//			// 是禁布区，且未被访问过
	//			NodePoint& neib_pt = (*m_ptArr)[neib_x][neib_y];
	//			if (neib_pt.symbol == obsSymbol && neib_pt.obsIndex == -1) {
	//				neib_pt.obsIndex = obsSum;
	//				extend_list.push_back(&neib_pt);
	//				obsList.push_back(&neib_pt);
	//			}
	//		}
	//	}
	//}
	//obsObj.m_pts = obsList;

	// 创建禁布区，添加第一层角节点，并对角节点沿轮廓排序，设置顺序值
	// 0.预处理，设置禁布区编号，放入新建的 openList 中，准备进行广度优先搜索
	std::vector<NodePoint*> extend_list;
	std::vector<NodePoint*> obsList;
	pt.obsIndex = obsSum;
	extend_list.emplace_back(&pt);		//搜索过程中的外围点
	obsList.emplace_back(&pt);		//禁布区所有点的集合
	this->m_obsArr.emplace_back(Obstacles(obsSum));
	Obstacles& obsObj = this->m_obsArr.back();	//第0个禁布区，暂时为空
	unordered_set<NodePoint*> no_cpt_set;
	// 1.将所有连在一起的禁布区点视作一个禁布区，将地图中点的这些点的 obsIndex 设为 -2，表示已探索过
	while (!extend_list.empty()) {
		NodePoint& pt_center = *extend_list.back();
		for (const auto& dir : four_corners) {
			int neib_x = pt_center.x + dir.x;
			int neib_y = pt_center.y + dir.y;
			if (is_inside(neib_x, neib_y)) {
				NodePoint& neb1 = (*m_ptArr)[neib_x][pt_center.y];
				NodePoint& neb2 = (*m_ptArr)[pt_center.x][neib_y];

				// 是禁布区，且未被访问过
				NodePoint& neib_pt = (*m_ptArr)[neib_x][neib_y];
				bool is_cornerObs = neb1.symbol == accessSymbol && neb2.symbol == accessSymbol;
				if (neib_pt.symbol == obsSymbol && is_cornerObs) {
					neb1.symbol = obsSymbol;
					neb2.symbol = obsSymbol;
					no_cpt_set.insert(&neb1);
					no_cpt_set.insert(&neb2);
				}
			}
		}
		extend_list.pop_back();
		for (const auto& dir : directions) {
			int neib_x = pt_center.x + dir.x;
			int neib_y = pt_center.y + dir.y;
			if (is_inside(neib_x, neib_y)) {
				// 是禁布区，且未被访问过
				NodePoint& neib_pt = (*m_ptArr)[neib_x][neib_y];
				if (neib_pt.symbol == obsSymbol && neib_pt.obsIndex == -1) {
					neib_pt.obsIndex = obsSum;
					extend_list.emplace_back(&neib_pt);
					obsList.emplace_back(&neib_pt);
				}
			}
		}
	}
	obsObj.m_pts = obsList;

	// 2.获取禁布区的外轮廓，并将外轮廓的 obsIndex 设为禁布区的编号
	for (auto xy_ptr : obsList) {
		bool is_outline = false;
		int neib_x;
		int neib_y;
		for (auto& dir : directions) {
			neib_x = xy_ptr->x + dir.x;
			neib_y = xy_ptr->y + dir.y;
			if (!is_inside(neib_x, neib_y) || (*m_ptArr)[neib_x][neib_y].symbol == accessSymbol) {
				// 出界，或者上下左右存在通行点，为禁布区轮廓点
				is_outline = true;
				break;
			}
		}
		if (is_outline) {
			xy_ptr->obsIndex = obsSum;
			obsObj.outline.emplace_back(xy_ptr);
			continue;
		}
		else {
			for (auto& dir : four_corners) {
				neib_x = xy_ptr->x + dir.x;
				neib_y = xy_ptr->y + dir.y;
				if ((*m_ptArr)[neib_x][neib_y].symbol == accessSymbol) {
					//出界会在前面continue处结束，此处必定不出界，四个角存在通行点
					xy_ptr->obsIndex = obsSum;
					obsObj.outline.emplace_back(xy_ptr);
					break;
				}
			}
		}
	}
	obsObj.index = obsSum;
	obsSum++;
	// 3.寻找首层节点的坐标，创建节点，并设置节点的外扩方向（不考虑两个禁布区对角相连的情况）
	NodePoint* first_cpt_pt_in_outline = nullptr;		//该禁布区的一个角点所对应的黑点，用于后面排序
	for (const auto& pt_ptr : obsObj.outline) {
		if (no_cpt_set.find(pt_ptr) != no_cpt_set.end())
			continue;
		int neib_x;
		int neib_y;
		std::vector<NodePoint*> neighbors;
		for (const auto& dir : directions) {
			neib_x = pt_ptr->x + dir.x;
			neib_y = pt_ptr->y + dir.y;
			if (is_inside(neib_x, neib_y) && (*m_ptArr)[neib_x][neib_y].obsIndex >= 0) {
				neighbors.emplace_back(&(*m_ptArr)[neib_x][neib_y]);
			}
		}
		if (neighbors.size() > 2) {
			continue;
		}
		else if (neighbors.size() == 2) {
			if(no_cpt_set.find(neighbors[0]) != no_cpt_set.end()&& no_cpt_set.find(neighbors[1]) != no_cpt_set.end()) {
				// 两个邻居都是虚构的障碍点，则当作没有邻居处理
				int corner_x1 = pt_ptr->x + 1;
				int corner_y1 = pt_ptr->y + 1;
				int corner_x2 = pt_ptr->x - 1;
				int corner_y2 = pt_ptr->y + 1;
				int corner_x3 = pt_ptr->x - 1;
				int corner_y3 = pt_ptr->y - 1;
				int corner_x4 = pt_ptr->x + 1;
				int corner_y4 = pt_ptr->y - 1;
				NodePoint direction1 = NodePoint(1, 1);
				NodePoint direction2 = NodePoint(-1, 1);
				NodePoint direction3 = NodePoint(-1, -1);
				NodePoint direction4 = NodePoint(1, -1);
				bool got_a_corner_cpt = set_new_cpt(corner_x1, corner_y1, direction1);
				got_a_corner_cpt = set_new_cpt(corner_x2, corner_y2, direction2) || got_a_corner_cpt;
				got_a_corner_cpt = set_new_cpt(corner_x3, corner_y3, direction3) || got_a_corner_cpt;
				got_a_corner_cpt = set_new_cpt(corner_x4, corner_y4, direction4) || got_a_corner_cpt;
				if (got_a_corner_cpt && first_cpt_pt_in_outline == nullptr)
					first_cpt_pt_in_outline = pt_ptr;		//first_cpt_pt_ptr只被赋值一次有效值
			}
			else {
				// 两个邻居都是真实的障碍点，则当作没有邻居处理
				NodePoint vec1 = NodePoint(pt_ptr->x - neighbors[0]->x, pt_ptr->y - neighbors[0]->y);
				NodePoint vec2 = NodePoint(pt_ptr->x - neighbors[1]->x, pt_ptr->y - neighbors[1]->y);
				if (vec1 * vec2 == 0) {		//垂直，是禁布区的角
					NodePoint direction0 = vec1 + vec2;
					int corner_x = pt_ptr->x + direction0.x;
					int corner_y = pt_ptr->y + direction0.y;
					bool got_a_corner_cpt = set_new_cpt(corner_x, corner_y, direction0);
					if (got_a_corner_cpt && first_cpt_pt_in_outline == nullptr)		// 是凸起的角
						first_cpt_pt_in_outline = pt_ptr;		//first_cpt_pt_ptr只被赋值一次有效值
				}
			}
		}
		else if (neighbors.size() == 1) {
			int dx = pt_ptr->x - neighbors[0]->x;
			int dy = pt_ptr->y - neighbors[0]->y;
			if (dx == 0) {		//竖直向量（x为行号，y为列号）
				int corner_x1 = pt_ptr->x + 1;
				int corner_y1 = pt_ptr->y + dy;
				int corner_x2 = pt_ptr->x - 1;
				int corner_y2 = pt_ptr->y + dy;
				NodePoint direction1 = NodePoint(1, dy);
				NodePoint direction2 = NodePoint(-1, dy);
				bool got_a_corner_cpt = set_new_cpt(corner_x1, corner_y1, direction1);
				got_a_corner_cpt = set_new_cpt(corner_x2, corner_y2, direction2) || got_a_corner_cpt;
				if (got_a_corner_cpt && first_cpt_pt_in_outline == nullptr)
					first_cpt_pt_in_outline = pt_ptr;		//first_cpt_pt_ptr只被赋值一次有效值
			}
			else {		//水平方量dy == 0
				int corner_x1 = pt_ptr->x + dx;
				int corner_y1 = pt_ptr->y + 1;
				int corner_x2 = pt_ptr->x + dx;
				int corner_y2 = pt_ptr->y - 1;
				NodePoint direction1 = NodePoint(dx, 1);
				NodePoint direction2 = NodePoint(dx, -1);
				bool got_a_corner_cpt = set_new_cpt(corner_x1, corner_y1, direction1);
				got_a_corner_cpt = set_new_cpt(corner_x2, corner_y2, direction2) || got_a_corner_cpt;
				if (got_a_corner_cpt && first_cpt_pt_in_outline == nullptr)
					first_cpt_pt_in_outline = pt_ptr;		//first_cpt_pt_ptr只被赋值一次有效值
			}
		}
		else {		//neighbors.size() == 0，独立的点，添加四个角点
			int corner_x1 = pt_ptr->x + 1;
			int corner_y1 = pt_ptr->y + 1;
			int corner_x2 = pt_ptr->x - 1;
			int corner_y2 = pt_ptr->y + 1;
			int corner_x3 = pt_ptr->x - 1;
			int corner_y3 = pt_ptr->y - 1;
			int corner_x4 = pt_ptr->x + 1;
			int corner_y4 = pt_ptr->y - 1;
			NodePoint direction1 = NodePoint(1, 1);
			NodePoint direction2 = NodePoint(-1, 1);
			NodePoint direction3 = NodePoint(-1, -1);
			NodePoint direction4 = NodePoint(1, -1);
			bool got_a_corner_cpt = set_new_cpt(corner_x1, corner_y1, direction1);
			got_a_corner_cpt = set_new_cpt(corner_x2, corner_y2, direction2) || got_a_corner_cpt;
			got_a_corner_cpt = set_new_cpt(corner_x3, corner_y3, direction3) || got_a_corner_cpt;
			got_a_corner_cpt = set_new_cpt(corner_x4, corner_y4, direction4) || got_a_corner_cpt;
			if (got_a_corner_cpt && first_cpt_pt_in_outline == nullptr)
				first_cpt_pt_in_outline = pt_ptr;		//first_cpt_pt_ptr只被赋值一次有效值
		}
	}
	// 4. 对禁布区首层角点的坐标进行排序，并设置顺序值
	// 4.1预处理，确定第一个红点（轮廓外的移动点）和黑点（轮廓上的移动点）
	if (first_cpt_pt_in_outline == nullptr)
		return;
	NodePoint* black_pt = first_cpt_pt_in_outline;
	NodePoint* red_pt0 = nullptr;	//起始点(轮廓外的角点)
	for (const auto& dir : four_corners) {
		int surround_x_temp = black_pt->x + dir.x;
		int surround_y_temp = black_pt->y + dir.y;
		if (is_inside(surround_x_temp, surround_y_temp)) {
			NodePoint* surround_pt = &(*m_ptArr)[surround_x_temp][surround_y_temp];
			if (surround_pt->isCpt) {
				red_pt0 = surround_pt;
				break;
			}
		}
	}
	NodePoint* red_pt = red_pt0;
	bool move_red = true;
	obsObj.cornerPts.emplace_back(&m_cpts[red_pt0]);
	// 4.2循环移位实现绕一圈进行排序，红点为轮廓外围的点，黑点为轮廓上的点
	while (true) {
		if (move_red) {
			// 假设 next_step 返回值类型为 std::vector<int>，如果不是需根据实际情况调整
			NodePoint* next_pt = next_step(red_pt, black_pt, accessSymbol);
			if (next_pt) {	//不是空指针
				red_pt = next_pt;
				if (red_pt->isCpt) {
					obsObj.cornerPts.emplace_back(&this->m_cpts[red_pt]);
				}
			}
			else {
				move_red = false;
				continue;
			}
		}
		else {
			NodePoint* next_pt = next_step(black_pt, red_pt, obsSymbol);
			if (next_pt) {
				black_pt = next_pt;
			}
			else {
				move_red = true;
				continue;
			}
		}
		if (red_pt == red_pt0) {
			obsObj.cornerPts.pop_back();
			break;
		}
	}
	// 4.3为排序后的节点设置序号值，将首层角节点整体存入
	int cpt_sum = obsObj.cornerPts.size();
	for (size_t i = 0; i < cpt_sum; i++) {
		int left = (i + cpt_sum - 1) % cpt_sum;
		obsObj.cornerPts[i]->m_pt_left = obsObj.cornerPts[(i + cpt_sum - 1) % cpt_sum];
		obsObj.cornerPts[i]->m_pt_right = obsObj.cornerPts[(i + 1) % cpt_sum];
		obsObj.cornerPts[i]->m_c_obsIndex = obsObj.index;
	}
}
bool Router::set_new_cpt(int x, int y, NodePoint& ex_direc) {	//给定坐标，生成一个新角点对象
	if (is_inside(x, y)) {
		NodePoint* corner_ptr = &(*m_ptArr)[x][y];
		if (corner_ptr->isCpt) {			//已经被建立为节点
			return true;
		}
		else if (corner_ptr->symbol == accessSymbol) {	//该位置为空白（不是节点）
			corner_ptr->isCpt = true;
			CPoint new_cpt(corner_ptr, ex_direc);	//建立为角点
			this->m_cpts.insert(std::make_pair(corner_ptr, new_cpt));		//节点放入列表中
			return true;
		}
	}
	return false;	//该顶点没有角点（出界、斜角是禁布点）
}
NodePoint* Router::next_step(NodePoint* step_point, NodePoint* scope_point, char target_symbol) {
	int try_step_times = 0;		//上下左右移动失败的次数，非通行点时加1
	NodePoint*& old_point = target_symbol == accessSymbol ? this->old_red_pt : this->old_black_pt;
	vector<NodePoint> directions_step = directions;
	for (const auto& dir : directions_step) {	//上下左右四个方向的点尝试
		int sur_x = step_point->x + dir.x;
		int sur_y = step_point->y + dir.y;
		if (old_point && sur_x == old_point->x && sur_y == old_point->y) {
			continue;	//该点是上一步
		}

		if (is_inside(sur_x, sur_y)) {		// 不出界
			NodePoint& sur_pt = (*m_ptArr)[sur_x][sur_y];
			if (sur_pt.symbol != target_symbol) {
				try_step_times++;
				continue;	//当前点颜色不匹配，且不是节点
			}
			if (sur_x < scope_point->x - 1 || sur_x > scope_point->x + 1 ||
				sur_y < scope_point->y - 1 || sur_y > scope_point->y + 1) {
				continue;	//不在中心点范围内
			}
			old_point = step_point;
			return &sur_pt;
		}
		else {	//出界
			if (target_symbol == accessSymbol) {	//红点移动出界
				if (sur_x < scope_point->x - 1 || sur_x > scope_point->x + 1 ||
					sur_y < scope_point->y - 1 || sur_y > scope_point->y + 1) {
					continue;	//不在范围内
				}
				else {
					out_old_red_pt.set_xy(step_point->x, step_point->y);
					old_point = &out_old_red_pt;
					out_red_pt.set_xy(sur_x, sur_y);
					return &out_red_pt;		//返回一个界外的红点
				}
			}
			else {//黑点移动出界
				try_step_times++;
			}
		}
	}
	if (try_step_times == 3) {		//4个领居中，有3个是颜色不匹配
		old_point = nullptr;
		return nullptr;
	}
	return nullptr;
}

// 寻路模块
bool Router::path_finding(NodePoint* pt1, NodePoint* pt2) {
	int node1 = getNewNode(*pt1);
	int node2 = getNewNode(*pt2);
	m_path_tree.emplace_back(node1);
	m_path.emplace_back(node2);
	this->leaf_nodes.insert(std::make_pair(m_nodes[node1].F, node1));
	int times = 0;	// 调试代码
	while (!leaf_nodes.empty()) {
		int min_node = leaf_nodes.begin()->second;	//选择预估代价最小的叶节点【选择】
		leaf_nodes.erase(leaf_nodes.begin());
		m_pass_obss = get_all_next_obs(*m_nodes[min_node].m_ptr, *m_nodes[node2].m_ptr);	//【探索】
		if (m_pass_obss.empty()) {	// 说明node1直接可以扩展到node2，将途径的节点全部添加为子节点，直达node2
			NodePoint* target = m_nodes[node2].m_ptr;
			target->ocupied = true;
			bool added = set_child_and_value(min_node, node2);		// 设置父子关系，找到了路径
			if (added)
				leaf_nodes.insert(std::make_pair(m_nodes[node2].F, node2));
		}
		else {
			node_extend(min_node, node2);	//【扩展】

			for (auto& obs_id : m_pass_obss) {
				m_obsArr[obs_id].is_free = true;	// 完成所有禁布区角点的扩展后，为禁布区解锁
			}
		}
		if (m_nodes[node2].m_parent_n != -1)	//终点的父节点不是空指针，说明已经找到路径
			break;
	}
	if (leaf_nodes.empty())
		return false;
	return true;
}

void Router::set_obs_and_access(NodePoint* pt1, NodePoint* pt2, char symbol) {
	if (pt1->obsIndex >= 0) {
		// 将该禁布区设置为不是禁布区
		for (auto& pt : m_obsArr[pt1->obsIndex].m_pts) {
			pt->symbol = symbol;	// 完成所有禁布区角点的扩展后，为禁布区解锁
		}
	}
	if (pt2->obsIndex >= 0) {
		// 将该禁布区设置为不是禁布区
		for (auto& pt : m_obsArr[pt2->obsIndex].m_pts) {
			pt->symbol = symbol;	// 完成所有禁布区角点的扩展后，为禁布区解锁
		}
	}
}
bool Router::node_extend(int node1_n, int node2_n) {
	// 将途径的所有禁布区的角点扩展为node1的子节点，并锁定该禁布区
	for (int i = 0; i < m_pass_obss.size(); i++) {
		if (m_pass_obss[i] == m_nodes[node1_n].obsIndex) {	// 自阻挡型禁布区+++++
			add_neibor_to_node(node1_n, m_nodes[node1_n].m_ptr);
		}
		else if (m_pass_obss[i] == m_nodes[node2_n].obsIndex) {	// 目标阻挡型禁布区++++++
			;
		}
		else if (m_pass_obss[i] >= 0) {					// 中间阻挡型禁布区
			add_cpt_to_node(node1_n, m_pass_obss[i]);
		}
	}

	//cout << "node_extend：" << m_cpts.size()<< endl;
	return true;
}
bool Router::add_neibor_to_node(int node1_n, NodePoint* ptr) {		// 待扩展的节点编号，该节点所处的角点
	CPoint* corner_pt = &m_cpts[ptr];
	NodePoint* left_pt = corner_pt->m_pt_left->m_pt_ptr;

	int next_obs_id = get_next_obs(*ptr, *left_pt);
	if (next_obs_id == -4) {
		;
	}
	else if (next_obs_id == -3) {
		if (left_pt->ocupied) {
			set_child_and_value(node1_n, left_pt->m_node_id);
		}
		else {
			left_pt->ocupied = true;
			int new_node_n = getNewNode(*left_pt, m_nodes[node1_n].obsIndex);	// 新建一个节点，返回其编号
			bool added = set_child_and_value(node1_n, new_node_n);		// 设置父子关系，成功返回true
			if (added)
				leaf_nodes.insert(std::make_pair(m_nodes[new_node_n].F, new_node_n));
		}
	}
	else if (next_obs_id >= 0 && next_obs_id != m_nodes[node1_n].obsIndex && m_obsArr[next_obs_id].is_free) {  // 被其他禁布区阻挡
		m_obsArr[next_obs_id].is_free = false;
		m_pass_obss.emplace_back(next_obs_id);
	}

	NodePoint* right_pt = corner_pt->m_pt_right->m_pt_ptr;

	next_obs_id = get_next_obs(*ptr, *right_pt);
	if (next_obs_id == -4) {
		;
	}
	else if (next_obs_id == -3) {
		if (right_pt->ocupied) {
			set_child_and_value(node1_n, right_pt->m_node_id);
		}
		else {
			right_pt->ocupied = true;
			int new_node_n = getNewNode(*right_pt, m_nodes[node1_n].obsIndex);	// 新建一个节点，返回其编号
			bool added = set_child_and_value(node1_n, new_node_n);		// 设置父子关系，成功返回true
			if (added)
				leaf_nodes.insert(std::make_pair(m_nodes[new_node_n].F, new_node_n));
		}
	}
	else if (next_obs_id >= 0 && next_obs_id != m_nodes[node1_n].obsIndex && m_obsArr[next_obs_id].is_free) {
		m_obsArr[next_obs_id].is_free = false;
		m_pass_obss.emplace_back(next_obs_id);
	}
	return true;
}
bool Router::add_cpt_to_node(int node1_n, int obsIndex) {
	for (auto& corner_pt : m_obsArr[obsIndex].cornerPts) {
		int next_obs_id = get_next_obs(*m_nodes[node1_n].m_ptr, *corner_pt->m_pt_ptr);
		if (next_obs_id == -4) {	// 途径了其他角点 
			;
		}
		else if (next_obs_id == -3) {	// 可抵达目标点
			if (corner_pt->m_pt_ptr->ocupied) {
				set_child_and_value(node1_n, corner_pt->m_pt_ptr->m_node_id);
				continue;
			}
			corner_pt->m_pt_ptr->ocupied = true;
			int new_node_n = getNewNode(*corner_pt->m_pt_ptr, obsIndex);	// 新建一个节点，返回其编号
			bool added = set_child_and_value(node1_n, new_node_n);		// 设置父子关系，成功返回true
			if (added)
				leaf_nodes.insert(std::make_pair(m_nodes[new_node_n].F, new_node_n));
		}
		else if (next_obs_id == m_nodes[node1_n].obsIndex) {	//自身阻挡型禁布区
			add_neibor_to_node(node1_n, m_nodes[node1_n].m_ptr);
		}
		else if (!m_obsArr[next_obs_id].is_free)
			continue;
		else {		// 中间阻挡型，且未被锁定
			m_obsArr[next_obs_id].is_free = false;
			m_pass_obss.emplace_back(next_obs_id);
		}
	}
	return true;
}
int Router::get_next_obs(NodePoint& pt1, NodePoint& pt2) {		// 返回下一个禁布区编号,途中经过其他角点返回-4，直接抵达返回-3
	NodePoint pt_temp = pt1;
	NodePoint* pt = &pt1;
	int steps = 1;
	while (pt_temp != pt2) {
		pt_temp = pt1.get_next_pt(pt2, steps);
		NodePoint& ptL = (*m_ptArr)[pt->x][pt_temp.y];
		NodePoint& ptR = (*m_ptArr)[pt_temp.x][pt->y];
		pt = &(*m_ptArr)[pt_temp.x][pt_temp.y];
		if (pt->symbol == obsSymbol) {		// 遇到禁布区，返回禁布区编号
			return pt->obsIndex;
		}
		//else if (ptL.symbol == obsSymbol) { // 斜走时的两侧
		//	return ptL.obsIndex;
		//}
		//else if (ptR.symbol == obsSymbol) {	// 斜走时的两侧
		//	return ptR.obsIndex;
		//}
		else if (*pt == pt2)	// 直接抵达目标点
			return -3;
		steps++;
	}
	return -3;
}
std::vector<int> Router::get_all_next_obs(NodePoint& pt1, NodePoint& pt2) {		// 返回下一个禁布区编号,途中经过其他角点返回-4，直接抵达返回-3
	NodePoint pt_temp = pt1;
	int steps = 1;
	std::vector<int> pass_obss;
	while (pt_temp != pt2) {
		pt_temp = pt1.get_next_pt(pt2, steps);
		NodePoint& pt = (*m_ptArr)[pt_temp.x][pt_temp.y];
		if (pt.symbol == obsSymbol) {
			if (pt.obsIndex >= 0 && m_obsArr[pt.obsIndex].is_free) {
				m_obsArr[pt.obsIndex].is_free = false;
				pass_obss.emplace_back(pt.obsIndex);
			}
		}
		steps++;
	}
	return pass_obss;
}

bool Router::set_child_and_value(int parent_n, int child_n, bool ignoreG) {
	// ignoreG默认为false，若传入true,要确保child_n已有父节点，且可以找到父子关系
	Node& parent = m_nodes[parent_n];
	Node& child = m_nodes[child_n];
	double G_temp = parent.G + parent.m_ptr->get_distance(child.m_ptr);
	if (child.G > 0 && G_temp >= child.G)
		return false;
	if (G_temp < child.G || ignoreG) {
		// 断开原本的父子关系
		Node& c_parent = m_nodes[child.m_parent_n];
		for (auto itr = c_parent.m_children.begin(); itr != c_parent.m_children.end(); itr++) {
			if ((*itr) == child_n) {
				c_parent.m_children.erase(itr);
				break;
			}
		}
		//更新子节点极其后续节点的FGH
	}
	// 将子节点添加给父节点，并更新FGH，并将子节点添加到leaf_nodes
	child.G = G_temp;
	child.H = child.m_ptr->get_distance(m_nodes[m_path.back()].m_ptr);
	child.F = child.G + child.H;
	child.m_parent_n = parent_n;
	parent.m_children.push_back(child_n);
	return true;
}
int Router::getNewNode(NodePoint& pt, int obsIndex) {
	pt.m_node_id = m_nodes.size();
	Node node = Node(m_nodes.size(), &pt);
	node.obsIndex = obsIndex;
	m_nodes.push_back(node);
	return m_nodes.size() - 1;
}
void Router::finding_data_clear(int root) {		// 清理中间节点，叶节点，搜索树的占领点
	m_mid_node_ptr = nullptr;
	leaf_nodes.clear();
	// 清理ocupied的点
	m_nodes[root].m_ptr->ocupied = false;
	free_ocupid_pts(root);
}
int Router::free_ocupid_pts(int root_n) {
	Node* root = &m_nodes[root_n];
	vector<int>& children = root->m_children;
	for (int child_n : children) {
		Node* child = &m_nodes[child_n];
		child->m_ptr->ocupied = false;
		free_ocupid_pts(child_n);
	}
	return 0;
}
int Router::set_tree_lines(int root_n) {
	Node* root = &m_nodes[root_n];
	vector<int> children = root->m_children;
	for (int child_n : children) {
		Node* child = &m_nodes[child_n];
		// 1.将当前节点与子节点连线
		m_tree_lines.back().emplace_back(LinePtr(root->m_ptr, child->m_ptr));
		// 2.递归调用
		set_tree_lines(child_n);
	}
	return 0;
}

//节点优化模块（每次调用处理一条线）
int Router::node_optimize(int node_n, int end_node_index) {		// 传入终点进行回溯，没有优化返回-1，有优化返回最前面的优化节点的id
	int node_n_backup = node_n;
	int node_id = -1;
	int parent_n_id_next_c = m_nodes[node_n].m_parent_n;
	bool dis_changed = true;
	while (parent_n_id_next_c != end_node_index) {
		int node_n_p = m_nodes[parent_n_id_next_c].m_parent_n;	// 原始节点的祖父节点
		if (node_n_p == -1)
			break;
		// 判断能否直达，能直达，则不移动原始节点，更新原始节点的父子关系，不能直达则移动原始节点，继续迭代
		vector<NodePoint*> mid_pts = get_mid_node(*m_nodes[node_n].m_ptr, *m_nodes[node_n_p].m_ptr);
		if (mid_pts.empty()) {
			bool update_child = set_child_and_value(node_n_p, node_n);
			if (update_child) {
				node_id = node_n;
				dis_changed = false;
			}
		}
		else if (mid_pts.back()->symbol == obsSymbol) {	// 不可抵达的点
			;
		}
		else {
			insert_node(node_n, node_n_p, mid_pts);
		}
		if (dis_changed) {	// 未改变，将原始节点更新为它的父节点，作为下一次的起点；下一次的起点更新为当前节点的父节点
			node_n = parent_n_id_next_c;		// 原始节点
			parent_n_id_next_c = m_nodes[node_n].m_parent_n;	//下一次的起点
		}
		else {	// 已改变，原始节点不变
			dis_changed = true;
			parent_n_id_next_c = m_nodes[node_n].m_parent_n;
		}
	}

	// 将途径的角点添加为节点
	dis_changed = true;
	while (dis_changed) {
		node_n = node_n_backup;
		dis_changed = false;
		while (node_n != -1) {
			if (end_node_index != -1 && node_n == m_nodes[end_node_index].m_parent_n)
				break;
			int node_n_p = m_nodes[node_n].m_parent_n;	// 父节点
			if (node_n_p == -1)
				break;
			vector<NodePoint*> mid_pts = get_mid_node(*m_nodes[node_n].m_ptr, *m_nodes[node_n_p].m_ptr);
			if (!mid_pts.empty() && mid_pts.back()->symbol != obsSymbol) {
				insert_node(node_n, node_n_p, mid_pts);
				dis_changed = true;;
			}
			node_n = node_n_p;
		}
	}
	return node_id;
}
int Router::get_n_parent(int node_n, int n) {	// 获取node_n的地n代父节点，n=0为父节点，n=1为祖父节点
	Node* node = &m_nodes[node_n];
	int n_parent = 0;
	while (n_parent++ < n) {
		int parent_id = node->m_parent_n;
		if (parent_id < 0)
			break;
		node = &m_nodes[parent_id];
	}
	return node->m_parent_n;
}
bool Router::insert_node(int node1_n, int node2_n, NodePoint* ptr) {	// 子节点，父节点，新节点
	if (ptr->ocupied) {		//该点已经被建立为节点并占领
		m_nodes[node1_n].G = 0;
		m_nodes[ptr->m_node_id].G = 0;
		set_child_and_value(node2_n, ptr->m_node_id, true);
		set_child_and_value(ptr->m_node_id, node1_n, true);
	}
	else {
		ptr->ocupied = true;
		int new_node_n = getNewNode(*ptr, node1_n);		// 新建一个节点，返回其编号
		m_nodes[node1_n].G = 0;
		set_child_and_value(node2_n, new_node_n);		// 设置父子关系，成功返回true
		set_child_and_value(new_node_n, node1_n, true);		// 设置父子关系，成功返回true
	}
	return true;
}
bool Router::insert_node(int node1_n, int node2_n, std::vector<NodePoint*> ptrs) {	// 子节点，父节点，连线上的多个新节点
	for (auto ptr : ptrs) {
		insert_node(node1_n, node2_n, ptr);
		node1_n = ptr->m_node_id;
	}
	return true;
}
vector<NodePoint*> Router::get_mid_node(NodePoint& pt1, NodePoint& pt2) {		// 返回下一个禁布区编号,途中经过其他角点返回-4，直接抵达返回-3
	vector<NodePoint*> result;
	NodePoint pt_temp = pt1;
	NodePoint* pt = &pt1;
	int steps = 1;
	while (pt_temp != pt2) {
		pt_temp = pt1.get_next_pt(pt2, steps);
		NodePoint& ptL = (*m_ptArr)[pt->x][pt_temp.y];
		NodePoint& ptR = (*m_ptArr)[pt_temp.x][pt->y];
		pt = &(*m_ptArr)[pt_temp.x][pt_temp.y];
		if (pt->isCpt && pt_temp != pt2) {		//途径了角点，放入列表，等待扩展
			result.push_back(pt);
		}
		else if (ptL.symbol == obsSymbol) {
			result.push_back(&ptL);
			return result;
		}
		else if (ptR.symbol == obsSymbol) {
			result.push_back(&ptR);
			return result;
		}
		else if (pt->symbol == obsSymbol) {	// 遇到障碍物，列表中放入障碍物点，作为判断依据
			result.push_back(pt);
			return result;
		}
		steps++;
	}
	return result;
}

//后处理模块（每次调用处理一条线）
bool Router::main_post_processing(int node_n) { //传入终点进行回溯
	int node1_n = node_n;		// 当前节点（子节点）
	int grand_child_n = node_n;
	while (m_nodes[node1_n].m_parent_n != -1) {
		int node2_n = m_nodes[node1_n].m_parent_n;			// 原始节点
		// 在两种处理方式中获取一种合适的方式，获得中间点,如果本来就是标准方向，则返回一个空指针
		std::vector<int> pass_obss;
		NodePoint* mid_pt = get_mid_pt(grand_child_n, node1_n, node2_n, pass_obss);
		if (mid_pt) {	// 非空指针，需要以mid_pt为参考调整走线
			bool fixed = line_post_process(node1_n, node2_n, mid_pt, pass_obss);
			if (!fixed) {		//未成功修复，说明插入了计划角点，需重新检测当前节点
				continue;
			}
		}
		// 是空指针，说明node1到node2走线是标准方向，检测下一条
		grand_child_n = node1_n;
		node1_n = m_nodes[node1_n].m_parent_n;
	}
	return true;
}
bool Router::line_post_process(int node1_n, int node2_n, NodePoint* mid_pt, std::vector<int>& pass_obss) {
	// 调整走线，已成功调整为标准方向，返回true，为绕障而插入计划角点，返回false
	NodePoint* ptr1 = m_nodes[node1_n].m_ptr;
	NodePoint* ptr2 = m_nodes[node2_n].m_ptr;
	if (pass_obss.empty()) {  // 调整走线，两段都不穿过禁布区
		insert_node(node1_n, node2_n, mid_pt);
		return true;
	}
	else {				// 调整走线会穿过禁布区，插入一个可以同时连接ptr1和ptr2的计划角点
		for (int obs_id : pass_obss) {
			m_obsArr[obs_id].is_free = false;
		}
		// 获取所有阻挡的禁布区
		int min_x = min(ptr1->x, ptr2->x);
		int max_x = max(ptr1->x, ptr2->x);
		int min_y = min(ptr1->y, ptr2->y);
		int	max_y = max(ptr1->y, ptr2->y);
		for (auto obs_id : pass_obss) {
			m_obsArr[obs_id].is_free = false;
		}
		for (int i = 0; i < pass_obss.size(); i++) {
			int obs_id = pass_obss[i];
			bool inserted = false;
			for (auto corner_pt : m_obsArr[obs_id].cornerPts) {
				if (*corner_pt->m_pt_ptr == *ptr1 || *corner_pt->m_pt_ptr == *ptr2)
					continue;
				NodePoint* ptr = corner_pt->m_pt_ptr;
				if (ptr->x <= max_x && ptr->x >= min_x && ptr->y <= max_y && ptr->y >= min_y) {	// 是范围内的计划角点
					if (ptr == ptr1)
						continue;
					int first_obs_id1 = get_next_obs(*ptr1, *ptr);
					if (first_obs_id1 == -3) {		// 调整插入计划角点，走线第一段不穿过禁布区
						int first_obs_id2 = get_next_obs(*ptr, *ptr2);
						if (first_obs_id2 == -3) {
							insert_node(node1_n, node2_n, ptr);
							inserted = true;
							break;
						}
						else if (m_obsArr[first_obs_id2].is_free) {
							m_obsArr[first_obs_id2].is_free = false;
							pass_obss.push_back(first_obs_id2);
						}
					}
					else if (m_obsArr[first_obs_id1].is_free) {		// 穿过其他禁布区，且其他禁布区未被锁定（考虑是自身所在的禁布区？）
						m_obsArr[first_obs_id1].is_free = false;
						pass_obss.push_back(first_obs_id1);
					}
				}
			}
			if (inserted)
				break;
		}
		for (auto obs_id : pass_obss) {
			m_obsArr[obs_id].is_free = true;
		}
		//node_optimize(node1_n_backup, node1_n);
		return false;
	}
}
NodePoint* Router::get_mid_pt(int node1_pre, int node1_n, int node2_n, std::vector<int>& pass_obss) {  // 前面一个节点，子节点，父节点
	int x1 = m_nodes[node1_n].m_ptr->x;
	int y1 = m_nodes[node1_n].m_ptr->y;
	int x2 = m_nodes[node2_n].m_ptr->x;
	int y2 = m_nodes[node2_n].m_ptr->y;
	int dx = x2 - x1;
	int dy = y2 - y1;
	int abs_dx = abs(dx);
	int abs_dy = abs(dy);
	if (abs_dx == 0 || abs_dy == 0 || abs_dx == abs_dy) {	// 水平、竖直、45度走线，不处理
		return nullptr;
	}
	// 前一条线的单位向量
	Pointf vec_pre = Pointf(x1 - m_nodes[node1_pre].m_ptr->x, y1 - m_nodes[node1_pre].m_ptr->y).normalize();
	// 后一条线的单位向量
	int node2_parent_n = m_nodes[node2_n].m_parent_n;
	Pointf vec_behind(0, 0);
	if (node2_parent_n >= 0)
		vec_behind = Pointf(m_nodes[node2_parent_n].m_ptr->x - x2, m_nodes[node2_parent_n].m_ptr->y - y2).normalize();
	int  mid_x1, mid_y1, mid_x2, mid_y2;	// 两种改线方式的中间点
	std::vector<int> pass_obss_1;
	std::vector<int> pass_obss_2;
	int pass_obs_sum1, pass_obs_sum2;		// 两种改线方式，穿过禁布区的数量
	double angl, ang2;
	if (abs_dy > abs_dx) {		// dy更大
		int step_y = dy > 0 ? 1 : -1;
		// 处理方式1，靠近起点走水平竖直线
		mid_x1 = x1;
		mid_y1 = y2 - abs_dx * step_y;
		Pointf vec1 = Pointf(mid_x1 - x1, mid_y1 - y1).normalize();	// 前单位向量
		Pointf vec2 = Pointf(x2 - mid_x1, y2 - mid_y1).normalize();	// 后单位向量
		angl = vec_pre * vec1 + vec2 * vec_behind;
		pass_obss_1 = get_all_next_obs(*m_nodes[node1_n].m_ptr, (*m_ptArr)[mid_x1][mid_y1]);
		pass_obs_sum1 = pass_obss_1.size();
		std::vector<int> temp = get_all_next_obs((*m_ptArr)[mid_x1][mid_y1], *m_nodes[node2_n].m_ptr);
		pass_obs_sum1 += temp.size();
		pass_obss_1.insert(pass_obss_1.end(), temp.begin(), temp.end());
		for (int obs_id : pass_obss_1) {
			m_obsArr[obs_id].is_free = true;
		}

		// 处理方式2，靠近终点走水平竖直线
		mid_x2 = x2;
		mid_y2 = y1 + abs_dx * step_y;
		vec1 = Pointf(mid_x2 - x1, mid_y2 - y1).normalize();	// 前单位向量
		vec2 = Pointf(x2 - mid_x2, y2 - mid_y2).normalize();	// 后单位向量
		ang2 = vec_pre * vec1 + vec2 * vec_behind;
		pass_obss_2 = get_all_next_obs(*m_nodes[node1_n].m_ptr, (*m_ptArr)[mid_x2][mid_y2]);
		pass_obs_sum2 = pass_obss_2.size();
		temp = get_all_next_obs((*m_ptArr)[mid_x2][mid_y2], *m_nodes[node2_n].m_ptr);
		pass_obs_sum2 += temp.size();
		pass_obss_2.insert(pass_obss_2.end(), temp.begin(), temp.end());
		for (int obs_id : pass_obss_2) {
			m_obsArr[obs_id].is_free = true;
		}
	}
	else {		// dx更大
		int step_x = dx > 0 ? 1 : -1;
		// 处理方式1，靠近起点走水平竖直线
		mid_y1 = y1;
		mid_x1 = x2 - abs_dy * step_x;
		Pointf vec1 = Pointf(mid_x1 - x1, mid_y1 - y1).normalize();
		Pointf vec2 = Pointf(x2 - mid_x1, y2 - mid_y1).normalize();
		angl = vec_pre * vec1 + vec2 * vec_behind;
		pass_obss_1 = get_all_next_obs(*m_nodes[node1_n].m_ptr, (*m_ptArr)[mid_x1][mid_y1]);
		pass_obs_sum1 = pass_obss_1.size();
		std::vector<int> temp = get_all_next_obs((*m_ptArr)[mid_x1][mid_y1], *m_nodes[node2_n].m_ptr);
		pass_obs_sum1 += temp.size();
		pass_obss_1.insert(pass_obss_1.end(), temp.begin(), temp.end());
		for (int obs_id : pass_obss_1) {
			m_obsArr[obs_id].is_free = true;
		}

		// 处理方式2，靠近终点走水平竖直线
		mid_y2 = y2;
		mid_x2 = x1 + abs_dy * step_x;
		vec1 = Pointf(mid_x2 - x1, mid_y2 - y1).normalize();
		vec2 = Pointf(x2 - mid_x2, y2 - mid_y2).normalize();
		ang2 = vec_pre * vec1 + vec2 * vec_behind;
		pass_obss_2 = get_all_next_obs(*m_nodes[node1_n].m_ptr, (*m_ptArr)[mid_x2][mid_y2]);
		pass_obs_sum2 = pass_obss_2.size();
		temp = get_all_next_obs((*m_ptArr)[mid_x2][mid_y2], *m_nodes[node2_n].m_ptr);
		pass_obs_sum2 += temp.size();
		pass_obss_2.insert(pass_obss_2.end(), temp.begin(), temp.end());
		for (int obs_id : pass_obss_2) {
			m_obsArr[obs_id].is_free = true;
		}
	}
	bool choose_1 = true;
	//if (pass_obs_sum1 * pass_obs_sum2 == 0 && pass_obs_sum1 + pass_obs_sum2 != 0) {		//有且仅有一种方式不穿过禁布区
	//	if (pass_obs_sum1 == 0)
	//		choose_1 = true;
	//	else
	//		choose_1 = false;
	//}
	//else {		// 两种走线都不穿过禁布区，或者都穿过禁布区
	//	if (angl > ang2)		// 取内积更大的方式（即向量夹角最小，也是走线夹角最大）
	//		choose_1 = true;
	//	else
	//		choose_1 = false;
	//}

	if (pass_obs_sum1 < pass_obs_sum2)		// 优先选择穿过禁布区最少的方式
		choose_1 = true;
	else if (pass_obs_sum1 > pass_obs_sum2)
		choose_1 = false;
	else if (angl > ang2)
		choose_1 = true;
	else
		choose_1 = false;



	if (choose_1) {
		pass_obss = pass_obss_1;
		return &(*m_ptArr)[mid_x1][mid_y1];
	}
	else {
		pass_obss = pass_obss_2;
		return &(*m_ptArr)[mid_x2][mid_y2];
	}
}